#! /usr/bin/env python
#
# Copyright (C) 2015, DFKI GmbH
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the DFKI GmbH nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#  Created on: 24.12.2015
#
#      Author: Martin Guenther <martin.guenther@dfki.de>

PACKAGE='sick_ldmrs_driver'
from dynamic_reconfigure.parameter_generator_catkin import *

from math import pi

gen = ParameterGenerator()

deg2rad = pi / 180
tics2rad = deg2rad / 32

scan_freq_enum = gen.enum([gen.const("ScanFreq1250", int_t, 0, "Scan frequency 12.5 Hz"),
                           gen.const("ScanFreq2500", int_t, 1, "Scan frequency 25.0 Hz"),
                           gen.const("ScanFreq5000", int_t, 2, "Scan frequency 50.0 Hz")],
                          "Available scan frequencies")

contour_enum = gen.enum([gen.const("ClosestPointOnly", int_t, 0, "Closest point only"),
                         gen.const("LowDensity",       int_t, 1, "Low density"),
                         gen.const("HighDensity",      int_t, 2, "High density")],
                         "Contour point density")

angular_res_enum = gen.enum([gen.const("FocusedRes",  int_t, 0, "Focused resolution"),
                             gen.const("ConstantRes", int_t, 1, "Constant resolution"),
                             gen.const("FlexRes",     int_t, 2, "Flexible resolution")],
                             "Angular resolution type")

range_reduction_enum = gen.enum([gen.const("RangeFull",         int_t, 0, "All layers full range"),
                                 gen.const("RangeLowerReduced", int_t, 1, "Lower 4 layers reduced range"),
                                 gen.const("RangeUpperReduced", int_t, 2, "Upper 4 layers reduced range"),
                                 gen.const("RangeAllReduced",   int_t, 3, "All 8 layers reduced range")],
                                 "Layer range reduction")

resolution_enum = gen.enum([gen.const("Res0125", int_t, 4,  "Angular resolution 0.125 degrees"),
                            gen.const("Res0250", int_t, 8,  "Angular resolution 0.25 degrees"),
                            gen.const("Res0500", int_t, 16, "Angular resolution 0.5 degrees"),
                            gen.const("Res1000", int_t, 32, "Angular resolution 1.0 degrees")],
                            "FlexRes angular resolution")


######  Name                     Type      Reconfiguration level             Description                                      Default           Min              Max

## ROS parameters
gen.add("frame_id",              str_t,    0, "The TF frame in which point clouds will be returned.",                         "ldmrs")

## Measurement parameters
gen.add("start_angle",           double_t, 0, "The angle of the first range measurement [rad].",                              1600 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)   # 50  deg
gen.add("end_angle",             double_t, 0, "The angle of the last range measurement [rad].",                              -1920 * tics2rad, -1920 * tics2rad, 1598 * tics2rad)   # -60 deg
gen.add("scan_frequency",        int_t,    0, "Scan frequency, 0 = 12.5Hz, 1 = 25 Hz, 2 = 50 Hz",                             0,                0,               2, edit_method=scan_freq_enum)
gen.add("sync_angle_offset",     double_t, 0, "Angle under which the LD-MRS measures at the time of the sync pulse [rad].",   0,               -5760 * tics2rad, 5759 * tics2rad)   # -180...179.96 deg
gen.add("angular_resolution_type", int_t,  0, "Angular resolution type: 0 = focused, 1 = constant, 2 = flexible",             1,                0,               2, edit_method=angular_res_enum)
gen.add("layer_range_reduction", int_t,    0, "0: Full range, 1: lower 4 reduced, 2: upper 4 reduced, 3: all reduced",        0,                0,               3, edit_method=range_reduction_enum)
gen.add("ignore_near_range",     bool_t,   0, "Ignore scan points up to 15m. Requires layer_range_reduction = lower 4 reduced.", False)
gen.add("sensitivity_control",   bool_t,   0, "Reduce the sensitivity automatically in case of extraneous light.",            False)

## FlexRes parameters
gen.add("flexres_start_angle1",  double_t, 0, "FlexRes: start angle of sector 1.",                                            1600 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle2",  double_t, 0, "FlexRes: start angle of sector 2.",                                            1120 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle3",  double_t, 0, "FlexRes: start angle of sector 3.",                                             960 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle4",  double_t, 0, "FlexRes: start angle of sector 4.",                                             640 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle5",  double_t, 0, "FlexRes: start angle of sector 5.",                                               0 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle6",  double_t, 0, "FlexRes: start angle of sector 6.",                                            -640 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle7",  double_t, 0, "FlexRes: start angle of sector 7.",                                            -960 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_start_angle8",  double_t, 0, "FlexRes: start angle of sector 8.",                                           -1280 * tics2rad, -1918 * tics2rad, 1600 * tics2rad)
gen.add("flexres_resolution1",   int_t,    0, "FlexRes: angular resolution of sector 1.",                                     32,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution2",   int_t,    0, "FlexRes: angular resolution of sector 2.",                                     16,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution3",   int_t,    0, "FlexRes: angular resolution of sector 3.",                                      8,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution4",   int_t,    0, "FlexRes: angular resolution of sector 4.",                                      4,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution5",   int_t,    0, "FlexRes: angular resolution of sector 5.",                                      8,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution6",   int_t,    0, "FlexRes: angular resolution of sector 6.",                                     16,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution7",   int_t,    0, "FlexRes: angular resolution of sector 7.",                                     32,               4,               32, edit_method=resolution_enum)
gen.add("flexres_resolution8",   int_t,    0, "FlexRes: angular resolution of sector 8.",                                     16,               4,               32, edit_method=resolution_enum)

## Object tracking parameters
gen.add("contour_point_density", int_t,    0, "Contour point density, 0: closest point only, 1: low density, 2: high density", 2,               0,               2, edit_method=contour_enum)
gen.add("min_object_age",        int_t,    0, "Minimum tracking age (number of scans) of an object to be transmitted.",        0,               0,               65535)
gen.add("max_prediction_age",    int_t,    0, "Maximum prediction age (number of scans) of an object to be transmitted.",      0,               0,               65535)
exit(gen.generate(PACKAGE, "sick_ldmrs_node", "SickLDMRSDriver"))
